#include <string>
#include "Rov.h"
using namespace math;


void fill(matrix<double>& m1, double f[]){
 for(int i = 0; i < (int)m1.RowNo(); i++)
    {
    for(int j = 0; j < (int)m1.ColNo(); j++){
      m1(i, j) = f[i*m1.ColNo() + j];
      }
    }
}

void skewsymm(matrix<double>& V, matrix<double> v){
	double f[] = {0, -v(2,0), v(1,0), v(2,0), 0, -v(0,0), -v(1,0), v(0,0), 0};
	fill(V, f);
}

void insert(matrix<double>& M, int row_mat_indx, int col_mat_indx, matrix<double> m){
 int row_start_indx = row_mat_indx*m.RowNo();
 int col_start_indx = col_mat_indx*m.ColNo();
 for(int i = row_start_indx; i < row_start_indx + (int)m.RowNo(); i++){
  for(int j = col_start_indx; j < col_start_indx + (int)m.ColNo(); j++){
   M(i,j) = m(i-row_start_indx, j-col_start_indx);
  }
 }
}

void fill(matrix<double>& M, matrix<double> mf[]){
 int inner_rown, inner_coln;
 inner_rown = mf[0].RowNo();
 inner_coln = mf[0].ColNo();
 int row_mat_n = M.RowNo() / inner_rown;
 int col_mat_n = M.ColNo() / inner_coln;

 for(int i = 0; i < row_mat_n; i++){
  for(int j = 0; j < col_mat_n; j++){
   insert(M, i, j, mf[i*col_mat_n + j]);
  }
 }
}

int sign(double d){
 if(d > 0) return 1;
 else if(d == 0) return 0;
 else return -1;
}

void cross(matrix<double>& v, matrix<double> v1, matrix<double> v2){
 double a,b,c,d,e,f;
 a = v1(0,0); b = v1(1,0); c = v1(2,0);
 d = v2(0,0); e = v2(1,0); f = v2(2,0);
 v(0,0) = (b*f - c*e);
 v(1,0) = (c*d - a*f);
 v(2,0) = (a*e - b*d);
}


CRov::CRov()
{
}

CRov::~CRov()
{
}

bool CRov::OnNewMail(MOOSMSG_LIST &NewMail)
{
	MOOSMSG_LIST::iterator iterMSG;
	for(iterMSG = NewMail.begin(); iterMSG != NewMail.end(); iterMSG++){
		CMOOSMsg & rMsg = *iterMSG;
		if(MOOSStrCmp(rMsg.GetKey(),"Propulsion"))
		{
	 		this->OnPropulsion(rMsg);
		}
	}
	return true;
}

bool CRov::OnConnectToServer()
{
		this->DoRegistrations();
        return true;
}
bool CRov::Iterate()
{
        return true;
}
bool CRov::OnStartUp()
{
	double dblu0;
	if(!this->m_MissionReader.GetConfigurationParam("u0", dblu0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf u0 missing\n");
	}

	double dblv0;
	if(!this->m_MissionReader.GetConfigurationParam("v0", dblv0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf v0 missing\n");
	}

	double dblw0;
	if(!this->m_MissionReader.GetConfigurationParam("w0", dblw0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf w0 missing\n");
	}

	double dblp0;
	if(!this->m_MissionReader.GetConfigurationParam("p0", dblp0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf p0 missing\n");
	}

	double dblq0;
	if(!this->m_MissionReader.GetConfigurationParam("q0", dblq0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf q0 missing\n");
	}

	double dblr0;
	if(!this->m_MissionReader.GetConfigurationParam("r0", dblr0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf r0 missing\n");
	}

	double dblx0;
	if(!this->m_MissionReader.GetConfigurationParam("x0", dblx0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf x0 missing\n");
	}

	double dbly0;
	if(!this->m_MissionReader.GetConfigurationParam("y0", dbly0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf y0 missing\n");
	}

	double dblz0;
	if(!this->m_MissionReader.GetConfigurationParam("z0", dblz0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf z0 missing\n");
	}

	double dblphi0;
	if(!this->m_MissionReader.GetConfigurationParam("phi0", dblphi0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf phi0 missing\n");
	}

	double dbltheta0;
	if(!this->m_MissionReader.GetConfigurationParam("theta0", dbltheta0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf theta0 missing\n");
	}

	double dblpsi0;
	if(!this->m_MissionReader.GetConfigurationParam("psi0", dblpsi0))
	{
		MOOSTrace("CRov::OnStartUp(): Conf psi0 missing\n");
	}

	double dblIx;
	if(!this->m_MissionReader.GetConfigurationParam("Ix", dblIx))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Ix missing\n");
	}

	double dblIy;
	if(!this->m_MissionReader.GetConfigurationParam("Iy", dblIy))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Iy missing\n");
	}

	double dblIz;
	if(!this->m_MissionReader.GetConfigurationParam("Iz", dblIz))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Iz missing\n");
	}

	double dblIxy;
	if(!this->m_MissionReader.GetConfigurationParam("Ixy", dblIxy))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Ixy missing\n");
	}

	double dblIyz;
	if(!this->m_MissionReader.GetConfigurationParam("Iyz", dblIyz))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Iyz missing\n");
	}

	double dblIxz;
	if(!this->m_MissionReader.GetConfigurationParam("Ixz", dblIxz))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Ixz missing\n");
	}

	double dblm;
	if(!this->m_MissionReader.GetConfigurationParam("m", dblm))
	{
		MOOSTrace("CRov::OnStartUp(): Conf m missing\n");
	}

	double dblxG;
	if(!this->m_MissionReader.GetConfigurationParam("xG", dblxG))
	{
		MOOSTrace("CRov::OnStartUp(): Conf xG missing\n");
	}

	double dblyG;
	if(!this->m_MissionReader.GetConfigurationParam("yG", dblyG))
	{
		MOOSTrace("CRov::OnStartUp(): Conf yG missing\n");
	}

	double dblzG;
	if(!this->m_MissionReader.GetConfigurationParam("zG", dblzG))
	{
		MOOSTrace("CRov::OnStartUp(): Conf zG missing\n");
	}

	double dblxB;
	if(!this->m_MissionReader.GetConfigurationParam("xB", dblxB))
	{
		MOOSTrace("CRov::OnStartUp(): Conf xB missing\n");
	}

	double dblyB;
	if(!this->m_MissionReader.GetConfigurationParam("yB", dblyB))
	{
		MOOSTrace("CRov::OnStartUp(): Conf yB missing\n");
	}

	double dblzB;
	if(!this->m_MissionReader.GetConfigurationParam("zB", dblzB))
	{
		MOOSTrace("CRov::OnStartUp(): Conf zB missing\n");
	}

	double dblXu;
	if(!this->m_MissionReader.GetConfigurationParam("Xu", dblXu))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Xu missing\n");
	}

	double dblYv;
	if(!this->m_MissionReader.GetConfigurationParam("Yv", dblYv))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Yv missing\n");
	}

	double dblZw;
	if(!this->m_MissionReader.GetConfigurationParam("Zw", dblZw))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Zw missing\n");
	}

	double dblKp;
	if(!this->m_MissionReader.GetConfigurationParam("Kp", dblKp))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Kp missing\n");
	}

	double dblMq;
	if(!this->m_MissionReader.GetConfigurationParam("Mq", dblMq))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Mq missing\n");
	}

	double dblNr;
	if(!this->m_MissionReader.GetConfigurationParam("Nr", dblNr))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Nr missing\n");
	}

	double dblXuu;
	if(!this->m_MissionReader.GetConfigurationParam("Xuu", dblXuu))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Xuu missing\n");
	}

	double dblYvv;
	if(!this->m_MissionReader.GetConfigurationParam("Yvv", dblYvv))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Yvv missing\n");
	}

	double dblZww;
	if(!this->m_MissionReader.GetConfigurationParam("Zww", dblZww))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Zww missing\n");
	}

	double dblKpp;
	if(!this->m_MissionReader.GetConfigurationParam("Kpp", dblKpp))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Kpp missing\n");
	}

	double dblMqq;
	if(!this->m_MissionReader.GetConfigurationParam("Mqq", dblMqq))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Mqq missing\n");
	}

	double dblNrr;
	if(!this->m_MissionReader.GetConfigurationParam("Nrr", dblNrr))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Nrr missing\n");
	}

	double dblXud;
	if(!this->m_MissionReader.GetConfigurationParam("Xud", dblXud))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Xud missing\n");
	}

	double dblYvd;
	if(!this->m_MissionReader.GetConfigurationParam("Yvd", dblYvd))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Yvd missing\n");
	}

	double dblZwd;
	if(!this->m_MissionReader.GetConfigurationParam("Zwd", dblZwd))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Zwd missing\n");
	}

	double dblKpd;
	if(!this->m_MissionReader.GetConfigurationParam("Kpd", dblKpd))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Kpd missing\n");
	}

	double dblMqd;
	if(!this->m_MissionReader.GetConfigurationParam("Mqd", dblMqd))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Mqd missing\n");
	}

	double dblNrd;
	if(!this->m_MissionReader.GetConfigurationParam("Nrd", dblNrd))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Nrd missing\n");
	}

	double dblkL;
	if(!this->m_MissionReader.GetConfigurationParam("kL", dblkL))
	{
		MOOSTrace("CRov::OnStartUp(): Conf kL missing\n");
	}

	double dblkD;
	if(!this->m_MissionReader.GetConfigurationParam("kD", dblkD))
	{
		MOOSTrace("CRov::OnStartUp(): Conf kD missing\n");
	}

	double dblkY;
	if(!this->m_MissionReader.GetConfigurationParam("kY", dblkY))
	{
		MOOSTrace("CRov::OnStartUp(): Conf kY missing\n");
	}

	double dblkl;
	if(!this->m_MissionReader.GetConfigurationParam("kl", dblkl))
	{
		MOOSTrace("CRov::OnStartUp(): Conf kl missing\n");
	}

	double dblkm;
	if(!this->m_MissionReader.GetConfigurationParam("km", dblkm))
	{
		MOOSTrace("CRov::OnStartUp(): Conf km missing\n");
	}

	double dblkn;
	if(!this->m_MissionReader.GetConfigurationParam("kn", dblkn))
	{
		MOOSTrace("CRov::OnStartUp(): Conf kn missing\n");
	}

	double dblClp;
	if(!this->m_MissionReader.GetConfigurationParam("Clp", dblClp))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Clp missing\n");
	}

	double dblClr;
	if(!this->m_MissionReader.GetConfigurationParam("Clr", dblClr))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Clr missing\n");
	}

	double dblCmq;
	if(!this->m_MissionReader.GetConfigurationParam("Cmq", dblCmq))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Cmq missing\n");
	}

	double dblCma;
	if(!this->m_MissionReader.GetConfigurationParam("Cma", dblCma))
	{
		MOOSTrace("CRov::OnStartUp(): Conf Cma missing\n");
	}

	double dblrho;
	if(!this->m_MissionReader.GetConfigurationParam("rho", dblrho))
	{
		MOOSTrace("CRov::OnStartUp(): Conf rho missing\n");
	}

	double dblA;
	if(!this->m_MissionReader.GetConfigurationParam("A", dblA))
	{
		MOOSTrace("CRov::OnStartUp(): Conf A missing\n");
	}

	double dbll;
	if(!this->m_MissionReader.GetConfigurationParam("l", dbll))
	{
		MOOSTrace("CRov::OnStartUp(): Conf l missing\n");
	}

	double dblh;
	if(!this->m_MissionReader.GetConfigurationParam("h", dblh))
	{
		MOOSTrace("CRov::OnStartUp(): Conf h missing\n");
	}

	double dblW;
	if(!this->m_MissionReader.GetConfigurationParam("W", dblW))
	{
		MOOSTrace("CRov::OnStartUp(): Conf W missing\n");
	}

	double dblB;
	if(!this->m_MissionReader.GetConfigurationParam("B", dblB))
	{
		MOOSTrace("CRov::OnStartUp(): Conf B missing\n");
	}

	this->_dblu0 = dblu0;
	this->_dblv0 = dblv0;
	this->_dblw0 = dblw0;
	this->_dblp0 = dblp0;
	this->_dblq0 = dblq0;
	this->_dblr0 = dblr0;
	this->_dblx0 = dblx0;
	this->_dbly0 = dbly0;
	this->_dblz0 = dblz0;
	this->_dblphi0 = dblphi0;
	this->_dbltheta0 = dbltheta0;
	this->_dblpsi0 = dblpsi0;

	this->_state.u = dblu0;
	this->_state.v = dblv0;
	this->_state.w = dblw0;
	this->_state.p = dblp0;
	this->_state.q = dblq0;
	this->_state.r = dblr0;
	this->_state.x = dblx0;
	this->_state.y = dbly0;
	this->_state.z = dblz0;
	this->_state.phi = dblphi0;
	this->_state.theta = dbltheta0;
	this->_state.psi = dblpsi0;

	this->_dblIx = dblIx;
	this->_dblIy = dblIy;
	this->_dblIz = dblIz;
	this->_dblIxy = dblIxy;
	this->_dblIyz = dblIyz;
	this->_dblIxz = dblIxz;
	this->_dblm = dblm;
	this->_dblxG = dblxG;
	this->_dblyG = dblyG;
	this->_dblzG = dblzG;
	this->_dblxB = dblxB;
	this->_dblyB = dblyB;
	this->_dblzB = dblzB;
	this->_dblXu = dblXu;
	this->_dblYv = dblYv;
	this->_dblZw = dblZw;
	this->_dblKp = dblKp;
	this->_dblMq = dblMq;
	this->_dblNr = dblNr;
	this->_dblXuu = dblXuu;
	this->_dblYvv = dblYvv;
	this->_dblZww = dblZww;
	this->_dblKpp = dblKpp;
	this->_dblMqq = dblMqq;
	this->_dblNrr = dblNrr;
	this->_dblXud = dblXud;
	this->_dblYvd = dblYvd;
	this->_dblZwd = dblZwd;
	this->_dblKpd = dblKpd;
	this->_dblMqd = dblMqd;
	this->_dblNrd = dblNrd;
	this->_dblkL = dblkL;
	this->_dblkD = dblkD;
	this->_dblkY = dblkY;
	this->_dblkl = dblkl;
	this->_dblkm = dblkm;
	this->_dblkn = dblkn;
	this->_dblClp = dblClp;
	this->_dblClr = dblClr;
	this->_dblCmq = dblCmq;
	this->_dblCma = dblCma;
	this->_dblrho = dblrho;
	this->_dblA = dblA;
	this->_dbll = dbll;
	this->_dblh = dblh;
	this->_dblW = dblW;
	this->_dblB = dblB;

	this->DoRegistrations();
    return true;
}

void CRov::DoRegistrations(){
	this->m_Comms.Register("Propulsion", 0);
	return;
}

/*
 * LIMITS
 *
 * */
bool inZRotSpeedLimit(double r){
	if(r < 0){
		r = r*-1;
	}
	return r <= 0.000005;
}

bool inXSpeedLimit(double u){
	if(u < 0){
		u = u*-1;
	}
	return u <= 0.000001;
}

bool inZSpeedLimit(double w){
	if(w < 0){
		w = w*-1;
	}
	return w <= 0.000001;
}

int brojac = 0;
bool CRov::OnPropulsion(CMOOSMsg& msg){

	//msg.Trace(); //print msg in details
	cout << "RCVD: " << msg.GetString() << endl;
	//play game
	matrix<double> v1_G(3,1);
	matrix<double> v2_G(3,1);

	double f1[] = {this->_state.u, this->_state.v, this->_state.w};
	fill(v1_G, f1);
	double f2[] = {this->_state.p, this->_state.q, this->_state.r};
	fill(v2_G, f2);
	//cout << "v1_G: " << v1_G << endl;
	//cout << "v2_G: " << v2_G << endl;
	//sleep(2);

	matrix<double> eta2_E(3,1);
	double f3[] = {this->_state.phi, this->_state.theta, this->_state.psi};
	fill(eta2_E, f3);
	//cout << "eta2_E: " << eta2_E << endl;
	//sleep(1);

	double U;
	U = sqrt(pow(this->_state.u,2) + pow(this->_state.v,2) + pow(this->_state.w,2));

	double alpha, beta;
	if(this->_state.u != 0){
		alpha = atan(this->_state.w / this->_state.u);
		beta = asin(this->_state.v / U);
	}else{
		alpha = 0;
		beta = 0;
	}

	double qq;
	qq = 0.5*this->_dblrho*pow(U,2);

	double c1,c2,c3,s1,s2,s3,t2;
	c1 = cos(this->_state.phi);
	c2 = cos(this->_state.theta);
	c3 = cos(this->_state.psi);
	s1 = sin(this->_state.phi);
	s2 = sin(this->_state.theta);
	s3 = sin(this->_state.psi);
	t2 = tan(this->_state.theta);

	matrix<double> J1(3,3);
	double f4[] = {
		(c3*c2), (c3*s2*s1 - s3*c1), (s3*s1 + c3*c1*s2),
		(s3*c2), (c1*c3 + s1*s2*s3), (c1*s2*s3 - c3*s1),
		(-s2), (c2*s1), (c1*c2)};
	fill(J1, f4);
	//cout << "J1: " << J1 << endl;
	//sleep(1);

	matrix<double> J2(3,3);
	double f5[] = {
		1, (s1*t2), (c1*t2),
		0, c1, -s1,
		0, (s1/c2), (c1/c2)};
	fill(J2, f5);
	//cout << "J2: " << J2 << endl;
	//sleep(1);

	matrix<double> rG(3,1);
	double f6[] = {
		this->_dblxG,
		this->_dblyG,
		this->_dblzG};
	fill(rG, f6);
	//cout << "rG: " << rG << endl;
	//sleep(1);

	matrix<double> S_v1_G(3,3), S_v2_G(3,3), S_rG(3,3);
	skewsymm(S_v1_G, v1_G);
	skewsymm(S_v2_G, v2_G);
	skewsymm(S_rG, rG);
	//cout << "S_v1_G: " << S_v1_G << endl;
	//cout << "S_v2_G: " << S_v2_G << endl;
	//cout << "S_rG: " << S_rG << endl;
	//sleep(1);

	matrix<double> crb1(3,3), crb2(3,3), crb3(3,3), crb4(3,3);
	crb2 = -this->_dblm*S_v1_G - this->_dblm*S_v2_G*S_rG;
	crb3 = -this->_dblm*S_v1_G + this->_dblm*S_rG*S_v2_G;

	matrix<double> I0(3,3);
	double f7[] = {
		this->_dblIx, -this->_dblIxy, -this->_dblIxz,
		-this->_dblIxy, this->_dblIy, -this->_dblIyz,
		-this->_dblIxz, -this->_dblIyz, this->_dblIz};
	fill(I0, f7);
	skewsymm(crb4, I0*v2_G);
	crb4 = -crb4;

	matrix<double> CRB(6,6);
	matrix<double> mf1[] = {crb1, crb2, crb3, crb4};
	fill(CRB, mf1);
	//cout << "CRB: " << CRB << endl;
	//sleep(1);

	matrix<double> M11(3,3), M12(3,3), M21(3,3), M22(3,3);
	M11(0,0) = this->_dblXud;
	M11(1,1) = this->_dblYvd;
	M11(2,2) = this->_dblZwd;
	M22(0,0) = this->_dblKpd;
	M22(1,1) = this->_dblMqd;
	M22(2,2) = this->_dblNrd;

	matrix<double> MA(6,6);
	matrix<double> mf2[] = {M11, M12, M21, M22};
	fill(MA, mf2);
	//cout << "MA: " << MA << endl;
	//sleep(1);

	matrix<double> ca1(3,3), ca2(3,3), ca3(3,3), ca4(3,3);
	skewsymm(ca2, M11*v1_G + M12*v2_G);
	ca2 = -ca2;
	skewsymm(ca3, M11*v1_G + M21*v2_G);
	ca3 = -ca3;
	skewsymm(ca4, M21*v1_G + M22*v2_G);
	ca4 = -ca4;

	matrix<double> CA(6,6);
	matrix<double> mf3[] = {ca1, ca2, ca3, ca4};
	fill(CA, mf3);
	//cout << "CA: " << CA << endl;
	//sleep(1);

	matrix<double> MRB(6,6);
	MRB(0,0) = this->_dblm;
	MRB(1,1) = this->_dblm;
	MRB(2,2) = this->_dblm;

	MRB(0,4) = this->_dblm * this->_dblzG;
	MRB(0,5) = -this->_dblm * this->_dblyG;
	MRB(1,3) = -this->_dblm * this->_dblzG;
	MRB(1,5) = this->_dblm * this->_dblxG;
	MRB(2,3) = this->_dblm * this->_dblyG;
	MRB(2,4) = -this->_dblm * this->_dblxG;

	MRB(3,1) = -this->_dblm * this->_dblzG;
	MRB(3,2) = this->_dblm * this->_dblyG;
	MRB(4,0) = this->_dblm * this->_dblzG;
	MRB(4,2) = -this->_dblm * this->_dblxG;
	MRB(5,0) = -this->_dblm * this->_dblyG;
	MRB(5,1) = this->_dblm * this->_dblxG;

	MRB(3,3) = this->_dblIx;
	MRB(3,4) = -this->_dblIxy;
	MRB(3,5) = -this->_dblIxz;
	MRB(4,3) = -this->_dblIxy;
	MRB(4,4) = this->_dblIy;
	MRB(4,5) = -this->_dblIyz;
	MRB(5,3) = -this->_dblIxz;
	MRB(5,4) = -this->_dblIyz;
	MRB(5,5) = this->_dblIz;
	//cout << "MRB: " << MRB << endl;
	//sleep(1);

	matrix<double> M(6,6), invM(6,6);
	M = MRB + MA;
	invM = !M;
	//cout << "M: " << M << endl;
	//sleep(1);

	matrix<double> C(6,6);
	C = CRB + CA;
	//cout << "C: " << C << endl;
	//sleep(1);

	matrix<double> D(6,6);
	D(0,0) = this->_dblXu + this->_dblXuu * abs(this->_state.u);
	D(1,1) = this->_dblYv + this->_dblYvv * abs(this->_state.v);
	D(2,2) = this->_dblZw + this->_dblZww * abs(this->_state.w);
	D(3,3) = this->_dblKp + this->_dblKpp * abs(this->_state.p);
	D(4,4) = this->_dblMq + this->_dblMqq * abs(this->_state.q);
	D(5,5) = this->_dblNr + this->_dblNrr * abs(this->_state.r);
	//cout << "D: " << D << endl;
	//sleep(1);

	double CL, CD, CY, Cl, Cm, Cn;
	CL = this->_dblkL*alpha*(180/M_PI);              // drag coefficient
	CD = this->_dblkD*abs(alpha)*(180/M_PI) + 0.02;  // lift coefficient
	CY = this->_dblkY*beta*(180/M_PI);               // side-force coefficient
	Cl = this->_dblkl*beta*(180/M_PI);               // roll coefficient
	Cm = this->_dblkm*alpha*(180/M_PI);              // pitch coefficient
	Cn = this->_dblkn*beta*(180/M_PI);               // yaw coefficient

	double L, Dd, fN, mL, Md, N;
	L  = -sign(this->_state.u)*qq*CL*this->_dblA;               // lift-force
	Dd = -sign(this->_state.u)*qq*CD*this->_dblA;             // drag-force
	fN = -qq*CY*this->_dblA;                       			// side-force
	mL = -qq*Cl*this->_dblA*this->_dbll;                     // roll moment
	Md = -qq*Cm*this->_dblA*this->_dblh;                     // pitch moment
	N  = -qq*Cn*this->_dblA*this->_dbll;                     // yaw moment

	matrix<double> d1(6,1);
	double f8[] = {
		-(Dd*cos(beta)*cos(alpha) - fN*cos(alpha)*sin(beta) - L*sin(alpha)),
		-(Dd*sin(beta) + fN*cos(beta)),
       -(Dd*cos(beta)*sin(alpha) - fN*sin(alpha)*sin(beta) + L*cos(alpha)),
                                    -mL,
                                    -Md,
                                    -N};
	fill(d1, f8);
	//cout << "d1: " << d1 << endl;
	//sleep(1);

	matrix<double> d2(6,1);
	double f9[] = {
		0,
		0,
        0,
		-0.25*this->_dblrho*U*this->_dblA*pow(this->_dbll,2)*(this->_dblClp*this->_state.p+this->_dblClr*this->_state.r),
        -0.25*this->_dblrho*U*this->_dblA*pow(this->_dblh,2)*(this->_dblCmq*this->_state.q),
        -0.25*this->_dblrho*U*this->_dblA*pow(this->_dbll,2)*(this->_dblClp*this->_state.p+this->_dblClr*this->_state.r)};
    fill(d2, f9);
	//cout << "d2: " << d2 << endl;
	//sleep(1);

	matrix<double> invJ1(3,3);
	invJ1 = !J1;

	matrix<double> fG_E(3,1), fG_G(3,1), fB_E(3,1), fB_G(3,1);
	fG_E(2,0) = this->_dblW;
	fG_G = invJ1*fG_E;
	fB_E(2,0) = -this->_dblB;
	fB_G = invJ1*fB_E;

	matrix<double> rB(3,1);
	rB(0,0) = this->_dblxB;
	rB(1,0) = this->_dblyB;
	rB(2,0) = this->_dblzB;

	matrix<double> g1(3,1), g2(3,1), crx1(3,1), crx2(3,1);
	g1 = -(fG_G+fB_G);
	cross(crx1,rG,fG_G);
	cross(crx2,rB,fB_G);
	g2 = -(crx1 + crx2);

	matrix<double> g(6,1);
	matrix<double> mf4[] = {g1, g2};
	fill(g, mf4);
	//cout << "g: " << g << endl;
	//sleep(1);

	matrix<double> nu(6,1);
	double f10[] = {
		this->_state.u,
		this->_state.v,
		this->_state.w,
		this->_state.p,
		this->_state.q,
		this->_state.r};
	fill(nu, f10);

	matrix<double> tau(6,1);
	double dblTau;
	string strAlloc = msg.GetString();

	MOOSValFromString (dblTau , strAlloc, "tau1", false);
	tau(0,0) = dblTau;
	MOOSValFromString (dblTau , strAlloc, "tau2", false);
	tau(1,0) = dblTau;
	MOOSValFromString (dblTau , strAlloc, "tau3", false);
	tau(2,0) = dblTau;
	MOOSValFromString (dblTau , strAlloc, "tau4", false);
	tau(3,0) = dblTau;
	MOOSValFromString (dblTau , strAlloc, "tau5", false);
	tau(4,0) = dblTau;
	MOOSValFromString (dblTau , strAlloc, "tau6", false);
	tau(5,0) = dblTau;

	matrix<double> nudot(6,1);
	nudot = -invM*C*nu - invM*(D*nu + d1 + d2) - invM*g + invM*tau;

	matrix<double> pom1(6,6), pom2(6,1), zero(3,3);
	matrix<double> mf5[] = {J1, zero, zero, J2};
	fill(pom1, mf5);
	matrix<double> mf6[] = {v1_G, v2_G};
	fill(pom2, mf6);

	matrix<double>	eta_E_dot(6,1);
	eta_E_dot = pom1 * pom2;

	double dblStep = 0.1;

	this->_state.u += nudot(0,0)*dblStep;
	if(inXSpeedLimit(this->_state.u)){
		this->_state.u = 0;
	}
	this->_state.v += nudot(1,0)*dblStep;
	this->_state.w += nudot(2,0)*dblStep;
	if(inZSpeedLimit(this->_state.w)){
		this->_state.w = 0;
	}
	this->_state.p += nudot(3,0)*dblStep;
	this->_state.q += nudot(4,0)*dblStep;
	this->_state.r += nudot(5,0)*dblStep;
	if(inZRotSpeedLimit(this->_state.r)){
		this->_state.r = 0.0;
	}
	this->_state.x += eta_E_dot(0,0)*dblStep;
	this->_state.y += eta_E_dot(1,0)*dblStep;
	this->_state.z += eta_E_dot(2,0)*dblStep;
	this->_state.phi += eta_E_dot(3,0)*dblStep;
	this->_state.theta += eta_E_dot(4,0)*dblStep;
	this->_state.psi += eta_E_dot(5,0)*dblStep;

	string strState;
	strState = MOOSFormat("u=%f,v=%f,w=%f,p=%f,q=%f,r=%f,x=%f,y=%f,z=%f,phi=%f,theta=%f,psi=%f",
		this->_state.u,
		this->_state.v,
		this->_state.w,
		this->_state.p,
		this->_state.q,
		this->_state.r,
		this->_state.x,
		this->_state.y,
		this->_state.z,
		this->_state.phi,
		this->_state.theta,
		this->_state.psi);

	cout << "SEND: " << strState << endl;
	this->m_Comms.Notify("State", strState, MOOSTime());

	return true;
}


